#include "servo_visual/visual_control.hpp"   // ✅ 放到包名路径下
#include <std_msgs/msg/float64.hpp>
#include <algorithm>
#include <vector>

namespace servo_visual {

static const rclcpp::Logger LOGGER = rclcpp::get_logger("visual_control_node");

// 构造
VisualControl::VisualControl()
: Node("visual_control_node"),
  simulation_phase_(0.0),
  control_active_(false)
{
  // 时钟
  clock_ = this->get_clock();

  // 初值
  position_reference_ = Eigen::Vector3d(0.0, 0.0, 0.0);
  velocity_ = Eigen::Vector3d::Zero();
  dt_ = 0.01;
  current_position_.x() = 0;
  current_position_.y() = 0;
  current_position_.z() = 0;
  current_visual_ = Eigen::Vector3d::Zero();
  result_data_ = geometry_msgs::msg::Point();
  simulation_step_ = 0.05;

  // --- 参数声明 ---
  this->declare_parameter<double>("target_x", 0.0);
  this->declare_parameter<double>("Kp_x", 0.015);
  this->declare_parameter<double>("error_threshold_x", 13.0);
  this->declare_parameter<double>("MAX_VEL_X", 0.8);

  this->declare_parameter<double>("target_y", 41.0);
  this->declare_parameter<double>("Kp_y", 0.005);
  this->declare_parameter<double>("error_threshold_y", 6.0);
  this->declare_parameter<double>("MAX_VEL_Y", 0.05);

  this->declare_parameter<double>("target_z", 83.0);
  this->declare_parameter<double>("Kp_z", 0.025);
  this->declare_parameter<double>("error_threshold_z", 7.0);
  this->declare_parameter<double>("MAX_VEL_Z", 0.12);

  // 取初值
  this->get_parameter("target_x", target_x_);
  this->get_parameter("Kp_x", Kp_x_);
  this->get_parameter("error_threshold_x", error_threshold_x_);
  this->get_parameter("MAX_VEL_X", MAX_VEL_X_);

  this->get_parameter("target_y", target_y_);
  this->get_parameter("Kp_y", Kp_y_);
  this->get_parameter("error_threshold_y", error_threshold_y_);
  this->get_parameter("MAX_VEL_Y", MAX_VEL_Y_);

  this->get_parameter("target_z", target_z_);
  this->get_parameter("Kp_z", Kp_z_);
  this->get_parameter("error_threshold_z", error_threshold_z_);
  this->get_parameter("MAX_VEL_Z", MAX_VEL_Z_);
  // --- 参数声明结束 ---

  // 订阅
  visual_sub_ = create_subscription<geometry_msgs::msg::Point>(
      "/camera_xyz_topic", 10,
      std::bind(&VisualControl::visual_callback, this, std::placeholders::_1));
  force_sub_  = create_subscription<geometry_msgs::msg::TwistStamped>(
      "/force_data", 10,
      std::bind(&VisualControl::force_callback, this, std::placeholders::_1));

  // TF
  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  // 发布 Twist
  twist_cmd_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>(
      "visual_control_node/delta_twist_cmds", 10);

  // 参数回调
  parameter_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&VisualControl::parameters_callback, this, std::placeholders::_1));

  // 时间
  last_msg_time_ = clock_->now();
}

void VisualControl::initialize() {
  setupPlanningSceneMonitor();
  setupServo();

  last_data_.x = 0.0;
  last_data_.y = 0.0;
  last_data_.z = 190.0;

  // 控制循环
  timer_ = create_wall_timer(
      std::chrono::milliseconds(100),
      std::bind(&VisualControl::control_loop, this));

  start_time_ = this->get_clock()->now();
  control_active_ = true;
  RCLCPP_INFO(this->get_logger(), "visual伺服节点已初始化，开始 10 秒闭环控制。");
}

void VisualControl::simulateCameraZ() {
  double center_z = 390.0;
  double amplitude_z = 30.0;
  result_data_.z = center_z + amplitude_z * std::sin(simulation_phase_);
  simulation_phase_ += simulation_step_;
  if (simulation_phase_ > 2 * M_PI) simulation_phase_ -= 2 * M_PI;
}

void VisualControl::visual_callback(const geometry_msgs::msg::Point::SharedPtr msg) {
  RCLCPP_INFO(this->get_logger(), "接收到视觉数据: X: %.3f, Y: %.3f, Z: %.3f",
              msg->x, msg->y, msg->z);
  result_data_.x = msg->x;
  result_data_.y = msg->y;
  result_data_.z = msg->z;

  current_visual_.x() = 0.05;
  current_visual_.y() = 0.0;
  current_visual_.z() = 0.0;

  last_data_ = *msg;
  last_msg_time_ = clock_->now();
}

void VisualControl::force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
  current_force_.x() = msg->twist.linear.x;
  current_force_.y() = msg->twist.linear.y;
  current_force_.z() = msg->twist.linear.z;
}

void VisualControl::control_loop() {
  double elapsed = (clock_->now() - start_time_).seconds();
  RCLCPP_INFO(this->get_logger(), "elapsed : %.2f 秒", elapsed);

  if (elapsed < 7.0) {
    // 0-7s：X/Y 闭环
    double dt = (clock_->now() - last_msg_time_).seconds();
    if (dt >= 0.5) {
      RCLCPP_WARN(this->get_logger(),
                  "已经 %.2f 秒没收到新的视觉消息, 速度设为零", dt);
      velocity_ = Eigen::Vector3d::Zero();
      last_msg_time_ = clock_->now();
    } else {
      xAxisClosedLoopControl();
      yAxisClosedLoopControl();
    }
    RCLCPP_INFO(this->get_logger(), "X/Y 控制进行中，已运行 %.2f 秒", elapsed);

    // 按 z 位置微调 target_y（注意：这里用 set_parameter，而不是 declare_parameter）
    if (elapsed > 6.0) {
      if (current_position_.z() > 300) {
        this->set_parameter(rclcpp::Parameter("target_y", 55.0));
        target_y_ = 55.0;
      } else if (current_position_.z() > 200) {
        this->set_parameter(rclcpp::Parameter("target_y", 48.0));
        target_y_ = 48.0;
      }
    }
  }
  else if (elapsed < 13.5) {
    // 7-13.5s：Z 闭环
    velocity_.x() = 0.0;
    velocity_.y() = 0.0;
    zAxisClosedLoopControl();
    RCLCPP_INFO(this->get_logger(), "Z闭环控制进行中，已运行 %.2f 秒", elapsed - 7.0);
  }
  else {
    // >13.5s：停止
    if (control_active_) {
      RCLCPP_INFO(this->get_logger(), "控制结束，停止运动");
      control_active_ = false;
      finished_ = true;
      timer_->cancel();
    }
    velocity_ = Eigen::Vector3d::Zero();
  }

  publishTwistCommand();
}

void VisualControl::publishTwistCommand() {
  geometry_msgs::msg::TwistStamped twist_msg;
  twist_msg.header.stamp = clock_->now();
  twist_msg.header.frame_id = "base_link";

  // 轴映射
  twist_msg.twist.linear.x = velocity_.z();
  twist_msg.twist.linear.y = velocity_.x();
  twist_msg.twist.linear.z = velocity_.y();
  twist_msg.twist.angular.x = 0.0;
  twist_msg.twist.angular.y = 0.0;
  twist_msg.twist.angular.z = 0.0;

  twist_cmd_pub_->publish(twist_msg);
}

void VisualControl::xAxisClosedLoopControl() {
  current_position_.x() = last_data_.x;
  double error_x = target_x_ - current_position_.x();
  velocity_.x() = -Kp_x_ * error_x;
  if (std::abs(error_x) < error_threshold_x_) {
    velocity_.x() = 0.0;
    RCLCPP_INFO(this->get_logger(), "[X闭] 误差 %.3f 小于阈值 %.3f，速度设为0", error_x, error_threshold_x_);
  } else {
    velocity_.x() = std::clamp(velocity_.x(), -MAX_VEL_X_, MAX_VEL_X_);
  }
  RCLCPP_INFO(this->get_logger(),
              "[X闭] 目标x=%.3f, 当前X=%.3f , 误差=%.3f => 速度X=%.3f",
              target_x_, current_position_.x(), error_x, velocity_.x());
}

void VisualControl::yAxisClosedLoopControl() {
  current_position_.y() = last_data_.y;
  double error_y = target_y_ - current_position_.y();
  velocity_.y() = Kp_y_ * error_y;
  if (std::abs(error_y) < error_threshold_y_) {
    velocity_.y() = 0.0;
    RCLCPP_INFO(this->get_logger(), "[Y闭] 误差 %.3f 小于阈值 %.3f，速度设为0", error_y, error_threshold_y_);
  } else {
    velocity_.y() = std::clamp(velocity_.y(), -MAX_VEL_Y_, MAX_VEL_Y_);
  }
  RCLCPP_INFO(this->get_logger(),
              "[Y闭] 目标y=%.3f, 当前Y=%.3f , 误差=%.3f, 速度Y=%.3f",
              target_y_, current_position_.y(), error_y, velocity_.y());
}

void VisualControl::zAxisClosedLoopControl() {
  current_position_.z() = last_data_.z;
  double error_z = target_z_ - current_position_.z();
  velocity_.z() = -Kp_z_ * error_z;
  if (std::abs(error_z) < error_threshold_z_) {
    velocity_.z() = 0.0;
    RCLCPP_INFO(this->get_logger(), "[Z闭] 误差 %.3f 小于阈值 %.3f，速度设为0", error_z, error_threshold_z_);
  } else {
    velocity_.z() = std::clamp(velocity_.z(), -MAX_VEL_Z_, MAX_VEL_Z_);
  }
  RCLCPP_INFO(this->get_logger(),
              "[Z闭] 目标z=%.3f, 当前Z=%.3f , 误差=%.3f => 速度Z=%.3f",
              target_z_, current_position_.z(), error_z, velocity_.z());
}

void VisualControl::smoothData() {
  static geometry_msgs::msg::Point filtered_data;
  double alpha = 0.8;

  filtered_data.x = alpha * filtered_data.x + (1.0 - alpha) * last_data_.x;
  filtered_data.y = alpha * filtered_data.y + (1.0 - alpha) * last_data_.y;
  filtered_data.z = alpha * filtered_data.z + (1.0 - alpha) * last_data_.z;

  last_data_ = filtered_data;
}

void VisualControl::setupPlanningSceneMonitor() {
  planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      shared_from_this(), "robot_description", tf_buffer_, "planning_scene_monitor");

  if (planning_scene_monitor_->getPlanningScene()) {
    planning_scene_monitor_->startStateMonitor("/joint_states");
    planning_scene_monitor_->setPlanningScenePublishingFrequency(25);
    planning_scene_monitor_->startPublishingPlanningScene(
        planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
        "/moveit_servo/publish_planning_scene");
    planning_scene_monitor_->startSceneMonitor();
    planning_scene_monitor_->providePlanningSceneService();
  } else {
    RCLCPP_ERROR(LOGGER, "规划场景配置失败");
    throw std::runtime_error("Planning scene setup failed");
  }
}

void VisualControl::setupServo() {
  auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(shared_from_this());
  if (!servo_parameters) {
    RCLCPP_FATAL(LOGGER, "无法加载伺服参数");
    throw std::runtime_error("Failed to load servo parameters");
  }
  servo_ = std::make_unique<moveit_servo::Servo>(shared_from_this(), servo_parameters, planning_scene_monitor_);
  servo_->start();
}

rcl_interfaces::msg::SetParametersResult
VisualControl::parameters_callback(const std::vector<rclcpp::Parameter> & parameters) {
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "success";

  for (const auto & param : parameters) {
    const std::string & name = param.get_name();
    RCLCPP_INFO(LOGGER, "参数 '%s' 发生改变，新值为: %s",
                name.c_str(), param.value_to_string().c_str());

    if (name == "target_x") target_x_ = param.as_double();
    else if (name == "Kp_x") Kp_x_ = param.as_double();
    else if (name == "error_threshold_x") error_threshold_x_ = param.as_double();
    else if (name == "MAX_VEL_X") MAX_VEL_X_ = param.as_double();
    else if (name == "target_y") target_y_ = param.as_double();
    else if (name == "Kp_y") Kp_y_ = param.as_double();
    else if (name == "error_threshold_y") error_threshold_y_ = param.as_double();
    else if (name == "MAX_VEL_Y") MAX_VEL_Y_ = param.as_double();
    else if (name == "target_z") target_z_ = param.as_double();
    else if (name == "Kp_z") Kp_z_ = param.as_double();
    else if (name == "error_threshold_z") error_threshold_z_ = param.as_double();
    else if (name == "MAX_VEL_Z") MAX_VEL_Z_ = param.as_double();
  }
  return result;
}

bool VisualControl::isFinished() const { return finished_; }

// 工厂函数（保持你原来的接口）
std::shared_ptr<VisualControl> create_and_initialize_visual_control() {
  auto node = std::make_shared<VisualControl>();
  node->initialize();
  return node;
}

namespace visual_control {
  void init() {
    rclcpp::init(0, nullptr);
    auto node = create_and_initialize_visual_control();
    rclcpp::spin(node);
    rclcpp::shutdown();
  }
}

} // namespace servo_visual
